# 内置库
import math
import os
import statistics

# 第三方库
import numpy as np
import matplotlib.pyplot as plt
from shapely.geometry import Point, Polygon
from typing import Dict, List, Tuple, Optional, Union
from hybrid_a_star.hybrid_a_star import hybrid_a_star_planning
from hybrid_a_star.hybrid_a_star import check_car_collision
import matplotlib.path as mpath


def plot_path_and_goal(path_planned, goal, nearest_point):
    path_planned = np.array(path_planned)
    path_x = path_planned[:, 0]
    path_y = path_planned[:, 1]
    goal_x = goal[0]
    goal_y = goal[1]
    near_x = nearest_point[0]
    near_y = nearest_point[1]
    plt.plot(path_x, path_y, color='purple', linewidth=4, label='Path', alpha=0.4)
    plt.scatter(goal_x, goal_y, color='red', label='Goal')
    plt.scatter(near_x, near_y, color='navy', label='Nearest Point')
    plt.legend()


def plot_dubins_pose(observation, color='r', s=1, ax=None):
    if ax is None:
        ax = plt.gca()  # 如果未提供ax参数，则使用当前的Axes对象
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].dubins_pose)):
        dubins_x = observation["hdmaps_info"]["tgsc_map"].dubins_pose[i]['x']
        dubins_y = observation["hdmaps_info"]["tgsc_map"].dubins_pose[i]['y']
        ax.scatter(dubins_x, dubins_y, color=color, s=s)


def plot_borderline(observation, linewidth=0.5, ax=None):
    if ax is None:
        ax = plt.gca()
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].borderline)):
        borderpoints = observation["hdmaps_info"]["tgsc_map"].borderline[i]['borderpoints']
        x_values = [point[0] for point in borderpoints]
        y_values = [point[1] for point in borderpoints]
        # ax.plot(x_values, y_values, linewidth=linewidth)
        ax.scatter(x_values, y_values, s=linewidth)


def plot_reference_path(observation, linewidth=0.5, ax=None):
    if ax is None:
        ax = plt.gca()
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].reference_path)):
        waypoints = observation["hdmaps_info"]["tgsc_map"].reference_path[i]['waypoints']
        x_values = [point[0] for point in waypoints]
        y_values = [point[1] for point in waypoints]
        ax.plot(x_values, y_values, linestyle='--', linewidth=linewidth)


def plot_goal(goal_x, goal_y, ax=None):
    if ax is None:
        ax = plt.gca()
    polygon = [[goal_x[0], goal_y[0]], [goal_x[1], goal_y[1]], [goal_x[2], goal_y[2]], [goal_x[3], goal_y[3]]]
    ax.fill(*zip(*polygon), color='y', alpha=0.4, label='goal')


def back_cars2_obstacles(x_1, y_1, local_x_range, local_y_range, image_ndarray, dist_1_start, value=False, distance=60):
    distance = dist_1_start if distance > dist_1_start else distance
    x_bit = (x_1 - local_x_range[0]) * 10
    y_bit = (y_1 - local_y_range[0]) * 10
    # 创建与image_ndarray相同形状的网格
    height, width = image_ndarray.shape
    y_indices, x_indices = np.meshgrid(np.arange(height), np.arange(width), indexing='ij')
    # 计算每个像素与给定点的笛卡尔距离
    distances = np.sqrt((y_indices - y_bit) ** 2 + (x_indices - x_bit) ** 2)
    # 找到与给定点距离为distance的所有索引
    indices = np.where(distances <= distance)
    # 创建原始数组的副本
    image_ndarray_copy = np.copy(image_ndarray)
    # 将对应索引位置的像素设置为False
    image_ndarray_copy[indices] = value
    return image_ndarray_copy


def back_cars2_obstacles_1(x_1, y_1, local_x_range, local_y_range, image_ndarray, dist_1_start, value=False, distance=20):
    x_bit = (x_1 - local_x_range[0]) * 10
    y_bit = (y_1 - local_y_range[0]) * 10
    # 创建原始数组的副本
    image_ndarray_copy = np.copy(image_ndarray)
    for i in range((int(x_bit)-10), int(x_bit+10)):
        for j in range((int(x_bit)-10), int(x_bit+10)):
            indices = (int(i),int(j))
            image_ndarray_copy[indices] = value
    # 将对应索引位置的像素设置为False
    return image_ndarray_copy

def start2_obstacles_1(x_1, y_1, local_x_range, local_y_range, image_ndarray, dist_1_start, value=True, distance=60):
    x_bit = (x_1 - local_x_range[0]) * 10
    y_bit = (y_1 - local_y_range[0]) * 10
    # 创建原始数组的副本
    image_ndarray_copy = np.copy(image_ndarray)
    for i in range((int(x_bit)-30), int(x_bit+30)):
        for j in range((int(x_bit)-30), int(x_bit+30)):
            indices = (int(i),int(j))
            image_ndarray_copy[indices] = value
    # 将对应索引位置的像素设置为False
    return image_ndarray_copy


def pursuit_yaw(path_planned):
    path_planned_1 = []
    for i in range(0, len(path_planned) - 1):
        curr_pt = path_planned[i]
        next_pt = path_planned[i + 1]

        angle2 = math.atan2(next_pt[1] - curr_pt[1], next_pt[0] - curr_pt[0])

        delta = angle2

        # 将偏航角赋值给path_planned_1中的点
        if len(curr_pt) == 4:
            path_planned_1.append([curr_pt[0], curr_pt[1], delta, curr_pt[3]])
        else:
            path_planned_1.append([curr_pt[0], curr_pt[1], delta, curr_pt[3], curr_pt[4]])

    # 复制倒数第二个点的偏航角到最后一个点
    path_planned_1.append(path_planned_1[-2][:])

    return path_planned_1


def plot_path_planned(observation, path_planned, start_x, start_y, goal_x, goal_y,
                      astar_path, x_min, x_max, y_min, y_max, x_1, y_1, spd_planned, scene_name):
    plot_borderline(observation, linewidth=0.01)
    plot_reference_path(observation, linewidth=0.5)
    plt.scatter(start_x, start_y, marker='o', color='b', s=20, label='start', facecolors='None')
    if 'vehicle_info' in observation and 1 in observation['vehicle_info']:
        plt.scatter(x_1, y_1, marker='o', color='y', s=20, label='other car', facecolors='None')
    plot_goal(goal_x, goal_y)
    if observation["test_setting"]["scenario_type"] == "loading" and hasattr(astar_path, "xlist"):
        plt.scatter(astar_path.xlist[0], astar_path.ylist[0], marker='o', color='m', s=15,
                    label='Astar start point', facecolors='None')

    if observation["test_setting"]["scenario_type"] == "unloading" and hasattr(astar_path, "xlist"):
        plt.scatter(astar_path.xlist[-1], astar_path.ylist[-1], marker='o', color='m', s=15,
                    label='Astar end point', facecolors='None')
    x_coords = [point[0] for point in path_planned]
    y_coords = [point[1] for point in path_planned]
    plt.scatter(x_coords, y_coords, c=[abs(x) for x in spd_planned], s=0.2, cmap='rainbow')
    plt.xlim(x_min, x_max)
    plt.ylim(y_min, y_max)
    plt.legend()
    plt.title(scene_name)
    plt.colorbar(label='Speed (m/s)')


def distance(point1, point2):
    return math.sqrt((point2[0] - point1[0]) ** 2 + (point2[1] - point1[1]) ** 2)

def is_point_in_polygon(px, py, poly_x, poly_y):
    """
    判断点 (px, py) 是否在由 (poly_x, poly_y) 定义的多边形内部。
    poly_x 和 poly_y 分别是多边形顶点的 x 和 y 坐标列表。
    """
    polygon = [(poly_x[i], poly_y[i]) for i in range(len(poly_x))]
    path = mpath.Path(polygon)
    return path.contains_point((px, py))

class Planner:
    """ego车的轨迹规划器.
    注:业界一般做法是 path planning + speed planning .
    """

    def __init__(self, observation):
        self._goal_x = statistics.mean(observation['test_setting']['goal']['x'])
        self._goal_y = statistics.mean(observation['test_setting']['goal']['y'])
        self._goal_yaw = observation['test_setting']['goal']['head'][0]
        self._observation = observation

    def process(self, collision_lookup, observation, traj, plan_all=False):
        x_1, y_1 = 100, 100
        image_ndarray_new = observation['hdmaps_info']['image_mask'].image_ndarray
        if 'vehicle_info' in observation and 1 in observation['vehicle_info']:
            local_x_range = observation['hdmaps_info']['image_mask'].bitmap_info['bitmap_mask_PNG']['UTM_info'][
                'local_x_range']
            local_y_range = observation['hdmaps_info']['image_mask'].bitmap_info['bitmap_mask_PNG']['UTM_info'][
                'local_y_range']

            image_ndarray = observation['hdmaps_info']['image_mask'].image_ndarray
            x_1 = observation['vehicle_info'][1]['x']
            y_1 = observation['vehicle_info'][1]['y']
            start_x = observation['vehicle_info']["ego"]["x"]
            start_y = observation['vehicle_info']["ego"]["y"]
            goal_x = observation['test_setting']["goal"]["x"]
            goal_y = observation['test_setting']["goal"]["y"]
            dist_1_start = math.sqrt((start_x - x_1) ** 2 + (start_y - y_1) ** 2) * 10

            if is_point_in_polygon(x_1, y_1, goal_x, goal_y):
                image_ndarray_new = image_ndarray
            else:
                image_ndarray_new = back_cars2_obstacles(x_1, y_1, local_x_range, local_y_range, image_ndarray,
                                                         dist_1_start=dist_1_start, value=False)
            # image_ndarray_new = back_cars2_obstacles(start_x, start_y, local_x_range, local_y_range, image_ndarray_new,
            #                                    dist_1_start=dist_1_start, value=True)

        scene_name = observation['test_setting']['scenario_name']
        goal_x = observation['test_setting']["goal"]["x"]
        goal_y = observation['test_setting']["goal"]["y"]
        start_x = observation['vehicle_info']["ego"]["x"]
        start_y = observation['vehicle_info']["ego"]["y"]

        utm_local_range = observation['hdmaps_info']['image_mask'].bitmap_local_info['utm_local_range']
        x_min, y_min, x_max, y_max = utm_local_range[0], utm_local_range[1], utm_local_range[2], utm_local_range[3]
        x_margin, y_margin = 20, 20
        x_min, x_max, y_min, y_max = x_min - x_margin, x_max + x_margin, y_min - y_margin, y_max + y_margin

        collision_ini = check_car_collision(goal_x, goal_y, [observation['test_setting']['goal']['head'][0]] * 4,
                                            collision_lookup, observation,
                                            image_ndarray_new)

        if collision_ini:
            goal_x += 10 * np.cos(observation['test_setting']['goal']['head'][0] + np.pi)
            goal_y += 10 * np.sin(observation['test_setting']['goal']['head'][0] + np.pi)
            collision_ini = check_car_collision(goal_x, goal_y, [observation['test_setting']['goal']['head'][0]] * 4,
                                                collision_lookup, observation,
                                                image_ndarray_new)

            goal_x = observation['test_setting']["goal"]["x"]
            goal_y = observation['test_setting']["goal"]["y"]
            extra_dist = 1
            if not collision_ini:
                collision_ini = True
                while collision_ini:
                    goal_x += 0.01 * np.cos(observation['test_setting']['goal']['head'][0] + np.pi)
                    goal_y += 0.01 * np.sin(observation['test_setting']['goal']['head'][0] + np.pi)
                    collision_ini = check_car_collision([np.mean(goal_x)], [np.mean(goal_y)],
                                                        [observation['test_setting']['goal']['head'][0]],
                                                        collision_lookup, observation,
                                                        image_ndarray_new)

                goal_x += extra_dist * np.cos(observation['test_setting']['goal']['head'][0] + np.pi)
                goal_y += extra_dist * np.sin(observation['test_setting']['goal']['head'][0] + np.pi)
            else:
                collision_ini = True
                while collision_ini:
                    goal_x += 0.01 * np.cos(observation['test_setting']['goal']['head'][0])
                    goal_y += 0.01 * np.sin(observation['test_setting']['goal']['head'][0])
                    collision_ini = check_car_collision([np.mean(goal_x)], [np.mean(goal_y)],
                                                        [observation['test_setting']['goal']['head'][0]],
                                                        collision_lookup, observation,
                                                        image_ndarray_new)

                goal_x += extra_dist * np.cos(observation['test_setting']['goal']['head'][0])
                goal_y += extra_dist * np.sin(observation['test_setting']['goal']['head'][0])

            if 'new_goal' not in observation['test_setting']:
                observation['test_setting']['new_goal'] = {}
            observation['test_setting']['new_goal']['x'] = goal_x
            observation['test_setting']['new_goal']['y'] = goal_y
            observation['test_setting']['new_goal']['head'] = observation['test_setting']['goal']['head']

            # plt.figure(figsize=(6.2, 5), dpi=400)
            # plot_path_planned(observation, [], start_x, start_y, goal_x, goal_y,
            #                   [], x_min, x_max, y_min, y_max, x_1, y_1, [], scene_name)
            # plt.savefig(file_path)
        """规划器主函数.
        注：该函数功能设计举例
        1) 进行实时轨迹规划;
        2) 路径、速度解耦方案:给出局部轨迹规划器的实时求解结果--待行驶路径、待行驶速度；

        输入:observation——环境信息;
        输出: 路径、速度解耦方案:给出局部轨迹规划器的实时求解--
            待行驶路径（离散点序列）、待行驶速度（与路径点序列对应的速度离散点序列）
        """
        # 设置目标速度
        target_speed = 7
        target_speed_backward = 10 / 3.6

        goal = [np.mean(goal_x),
                np.mean(goal_y),
                observation['test_setting']['goal']['head'][0]]
        spd_planned = []
        # path_planned = self.get_ego_reference_path_to_goal(observation)
        keypoint_ind = 0
        if observation["test_setting"]["scenario_type"] == "loading" and not plan_all:
            path_planned = self.get_main_road(observation)
            sorted_path = [path_planned[0]]  # 将起点加入已排序路径中
            while len(sorted_path) < len(path_planned):
                # 找出离已排序路径的最后一个点最近的点
                nearest_point = min((point for point in path_planned if point not in sorted_path),
                                    key=lambda point: distance(sorted_path[-1], (point[0], point[1])))
                sorted_path.append(nearest_point)
            path_planned = sorted_path
            print('len(main_road)', len(path_planned))
            nearest_point, point_ind, min_distance, _ = self.find_nearest_point(path_planned, goal)
            print("min_distance:", min_distance)
            print("nearest_point", nearest_point)
            dist = 0
            while point_ind:
                dist += distance(path_planned[point_ind], path_planned[point_ind - 1])
                point_ind -= 1
                if dist > 30:
                    break
            path_planned = path_planned[0:point_ind + 1]
            # print("len(path_planned):",len(path_planned))
            # keypoint_ind = len(path_planned)  # keypoint_ind记录关键点的索引
            # print("keypoint:",keypoint)
            for i in range(len(path_planned)):
                path_planned[i] = path_planned[i][0:3]  # 将路径中的每个点的维度截断为3，添加一个布尔值True，表示路径上的关键点
                path_planned[i].append(True)
            handover_index = -1
            while handover_index >= -len(path_planned):
                handover = path_planned[handover_index]
                try:
                    astar_path = hybrid_a_star_planning(handover, goal, collision_lookup, observation, 2.0, 15, True,
                                                        image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1)
                    if astar_path == ([], [], []):
                        astar_path = hybrid_a_star_planning(handover, goal, collision_lookup, observation, 2.0, 15,
                                                            True,
                                                            image_ndarray_new, iter_number=1000, MOTION_RESOLUTION=0.1,
                                                            max_steer=50)
                    if hasattr(astar_path, "xlist"):
                        break
                except:
                    pass
                handover_index -= 50
                if handover_index <= -len(path_planned):
                    handover_index = -len(path_planned)
            path_planned = path_planned[0:len(path_planned) + handover_index]
            print("len(path_planned):", len(path_planned))
            keypoint_ind = len(path_planned)  # keypoint_ind记录关键点的索引
            for j in range(1, len(astar_path.xlist)):
                path_planned.append(
                    [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
                # if j+1 == len(astar_path.xlist):
                #     keypoint += j-1
                if astar_path.directionlist[j] == True and astar_path.directionlist[j + 1] == False:
                    print("keypoint:", [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                                        astar_path.directionlist[j]])
                    keypoint_ind += j - 1  # 将混合A*算法生成的路径添加到主要道路路径后面，并更新关键点的索引
                    # print("keypoint:",keypoint)
            print("keypoint_ind:", keypoint_ind)
            print("len(path):", len(path_planned))
            path_planned[keypoint_ind][-1] = 2
            if len(path_planned)>=2000:
                target_speed_f=15
                k_num = 600
            else:
                target_speed_f = target_speed
                k_num = 50
            for k in range(len(path_planned)):
                if k < k_num:
                    spd_planned.append(target_speed_f / 50 * 50)
                elif k >= k_num and k <= keypoint_ind - 100:
                    spd_planned.append(target_speed)
                elif (k > keypoint_ind - 100) and (k <= keypoint_ind):
                    spd_planned.append(target_speed / 100 * (keypoint_ind - k))
                elif k > keypoint_ind and k <= keypoint_ind + 100:
                    spd_planned.append(-target_speed_backward / 120 * (k - keypoint_ind + 20))
                elif k > keypoint_ind + 100 and k < len(path_planned) - 100:
                    spd_planned.append(-target_speed_backward)
                elif k >= len(path_planned) - 100:
                    spd_planned.append(-target_speed_backward / 100 * (len(path_planned) - k - 1))

        # elif observation["test_setting"]["scenario_type"] == "loading" and plan_all:
        #     astar_star = [observation['vehicle_info']['ego']['x'],
        #                   observation['vehicle_info']['ego']['y'],
        #                   observation['vehicle_info']['ego']['yaw_rad']]
        #     astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,image_ndarray_new)
        #     path_planned = []
        #     for j in range(1, len(astar_path.xlist)):
        #         path_planned.append(
        #             [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
        #         if astar_path.directionlist[j] == True and astar_path.directionlist[j + 1] == False:
        #             print("keypoint:", [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
        #                                 astar_path.directionlist[j]])
        #             keypoint_ind += j - 1  # 将混合A*算法生成的路径添加到主要道路路径后面，并更新关键点的索引
        #     path_planned[keypoint_ind][-1] = 2
        #     for k in range(len(path_planned)):
        #         if k < 50:
        #             spd_planned.append(target_speed / 50 * k)
        #         elif k >= 50 and k <= keypoint_ind - 100:
        #             spd_planned.append(target_speed)
        #         elif (k > keypoint_ind - 100) and (k <= keypoint_ind):
        #             spd_planned.append(target_speed / 100 * (keypoint_ind - k))
        #         elif k > keypoint_ind and k <= keypoint_ind + 100:
        #             spd_planned.append(-target_speed_backward / 120 * (k - keypoint_ind + 20))
        #         elif k > keypoint_ind + 100 and k < len(path_planned) - 100:
        #             spd_planned.append(-target_speed_backward)
        #         elif k >= len(path_planned) - 100:
        #             spd_planned.append(-target_speed_backward / 100 * (len(path_planned) - k - 1))

        elif observation["test_setting"]["scenario_type"] == "unloading":
            goal = [np.mean(observation['test_setting']['goal']['x']),
                    np.mean(observation['test_setting']['goal']['y']),
                    observation['test_setting']['goal']['head'][0]]
            path_planned = self.get_main_road(observation, False)
            astar_star = [observation['vehicle_info']['ego']['x'],
                          observation['vehicle_info']['ego']['y'],
                          observation['vehicle_info']['ego']['yaw_rad']]
            nearest_point, point_ind, min_distance, flag = self.find_nearest_point(path_planned, astar_star)
            if flag:
                astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
                                                    image_ndarray_new, iter_number=10000, MOTION_RESOLUTION=1)
                if astar_path == ([], [], []):
                    astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
                                                        image_ndarray_new, iter_number=10000, MOTION_RESOLUTION=1,
                                                        max_steer=50)

                path_planned = []
                for j in range(1, len(astar_path.xlist)):
                    path_planned.append(
                        [astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j], astar_path.directionlist[j]])
                for k in range(len(path_planned)):
                    if k < 50:
                        spd_planned.append(target_speed / 50 * k)
                    else:
                        spd_planned.append(target_speed)
            else:
                dist = 0
                while point_ind < len(path_planned):

                    dist += distance(path_planned[point_ind], path_planned[point_ind - 1])
                    point_ind += 1
                    # print(path_planned[point_ind+1][0])
                    if dist > 20:
                        break
                print("min_distance:", min_distance)
                print("nearest_point", nearest_point)
                path_planned = path_planned[point_ind - 1:]

                if len(path_planned) < 200:
                    handover = path_planned[-1]
                    astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0, 15,
                                                        False, image_ndarray_new, iter_number=100000,
                                                        MOTION_RESOLUTION=0.1)
                    if astar_path == ([], [], []):
                        astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0,
                                                            15, False, image_ndarray_new, iter_number=100000,
                                                            MOTION_RESOLUTION=0.1,
                                                            max_steer=50)
                    path_planned = []
                    path_planned1 = []
                    for j in range(1, len(astar_path.xlist)):
                        path_planned1.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                                              astar_path.directionlist[j]])
                    path_planned = path_planned1 + path_planned
                else:
                    handover_index = 0
                    # while handover_index <= len(path_planned):
                    #     handover = path_planned[handover_index]
                    #     try:
                    #         astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0, 15,False,image_ndarray_new,iter_number=100000,MOTION_RESOLUTION=0.1)
                    #         if hasattr(astar_path, "xlist"):
                    #             break
                    #     except:
                    #         pass
                    #     handover_index += 50
                    # path_planned = path_planned[len(path_planned)-handover_index:]
                    handover = path_planned[handover_index]
                    astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0, 15,
                                                        False, image_ndarray_new, 10000, 0.1)
                    if astar_path == ([], [], []):
                        astar_path = hybrid_a_star_planning(astar_star, handover, collision_lookup, observation, 2.0,
                                                            15, False, image_ndarray_new, 10000, 0.1,
                                                            max_steer=50)

                    for i in range(len(path_planned)):
                        path_planned[i] = path_planned[i][0:3]
                        path_planned[i].append(True)
                    path_planned1 = []
                    for j in range(1, len(astar_path.xlist)):
                        path_planned1.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                                              astar_path.directionlist[j]])
                    path_planned = path_planned1 + path_planned

                if len(path_planned) == 1755:
                    path_planned = []
                    i = "0.0"
                    # print(i)
                    backvehicle_traj = traj[1]
                    while True:
                        xback = backvehicle_traj[i]['x']
                        yback = backvehicle_traj[i]['y']
                        image_ndarray_new = back_cars2_obstacles_1(xback, yback, local_x_range, local_y_range,
                                                                   image_ndarray_new,
                                                                   dist_1_start=dist_1_start, value=False)
                        result = float(i) + 0.1
                        i = str(round(result, 1))
                        if ((result * 10) > (len(backvehicle_traj) - 2)):
                            image_ndarray_new = start2_obstacles_1(start_x, start_y, local_x_range, local_y_range,
                                                                   image_ndarray_new,
                                                                   dist_1_start=dist_1_start, value=True)
                            break
                    astar_path = hybrid_a_star_planning(astar_star, goal, collision_lookup, observation, 2.0, 15, True,
                                                        image_ndarray_new, iter_number=10000, MOTION_RESOLUTION=1,
                                                        max_steer=50)
                    for j in range(1, len(astar_path.xlist)):
                        path_planned.append([astar_path.xlist[j], astar_path.ylist[j], astar_path.yawlist[j],
                                             astar_path.directionlist[j]])

                if 'vehicle_info' in observation and 1 in observation['vehicle_info'] and dist_1_start < 300:
                    for k in range(len(path_planned)):
                        if k < 100:
                            spd_planned.append(target_speed / 100 * k / 3)
                        elif 100 <= k < 150:
                            spd_planned.append(target_speed / 50 * (k - 100) * 2 / 3 + target_speed / 3)
                        else:
                            spd_planned.append(target_speed)
                else:
                    for k in range(len(path_planned)):
                        if k < 50:
                            spd_planned.append(target_speed / 50 * k)
                        else:
                            spd_planned.append(target_speed)
        for i in range(1, len(path_planned)):
            path_planned[i].append(
                self.calculate_curvature(path_planned[i - 1][0], path_planned[i - 1][1], path_planned[i - 1][2],
                                         path_planned[i][0], path_planned[i][1], path_planned[i][2]))

        dir_current_file = os.path.dirname(os.path.dirname(__file__))
        file_path = os.path.join(os.path.dirname(dir_current_file), "all_results", "plot", scene_name + ".png")
        os.makedirs(os.path.dirname(file_path), exist_ok=True)

        plt.figure(figsize=(6.2, 5), dpi=400)
        plot_path_planned(observation, path_planned, start_x, start_y, goal_x, goal_y,
                          astar_path, x_min, x_max, y_min, y_max, x_1, y_1, spd_planned, scene_name)
        plt.savefig(file_path)

        return path_planned[1:], spd_planned[1:]

    def get_main_road(self, observation, bo_in=True):
        """获取HD Map中ego车到达铲装平台的参考路径.
        输入:observation——环境信息;
        输出:ego车到达目标区域的参考路径(拼接后).
        """
        main_road = {"polygon-14": [["path-2", "path-3", "path-4", "path-48", "path-50", "path-43", "path-44"],
                                    ["path-13", "path-14", "path-58", "path-31", "path-37", "path-38", "path-39"]],
                     "polygon-27": [
                         ["path-36", "path-5", "path-6", "path-7", "path-8", "path-9", "path-70", "path-71", "path-72",
                          "path-73"],
                         ["path-17", "path-18", "path-19", "path-20", "path-21", "path-65", "path-66", "path-67",
                          "path-64", "path-80", "path-81"]],
                     "polygon-29": [
                         ["path-36", "path-5", "path-6", "path-7", "path-8", "path-9", "path-84", "path-85", "path-74",
                          "path-75"],
                         ["path-22", "path-23", "path-59", "path-65", "path-66", "path-67", "path-64", "path-80",
                          "path-81"]],
                     "polygon-25": [["path-36", "path-5", "path-6", "path-7", "path-8", "path-9", "path-68", "path-69"],
                                    ["path-15", "path-16", "path-65", "path-66", "path-67", "path-64", "path-80",
                                     "path-81"]],
                     "polygon-10": [["path-2", "path-45", "path-46", "path-47", "path-40", "path-41"],
                                    ["path-77", "path-10", "path-11", "path-27", "path-28", "path-25", "path-26",
                                     "path-39"]]}
        #################更新参数#################
        self._ego_x = observation['vehicle_info']['ego']['x']
        self._ego_y = observation['vehicle_info']['ego']['y']
        self._ego_v = observation['vehicle_info']['ego']['v_mps']
        self._ego_yaw = observation['vehicle_info']['ego']['yaw_rad']
        #################定位主车和目标点所在几何#################
        if bo_in:
            ego_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._ego_x,
                                                                                                    self._ego_y)
            ego_polygon_id = int(ego_polygon_token.split('-')[1])
            print("ego_polygon_token:", ego_polygon_token)
            goal_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._goal_x,
                                                                                                     self._goal_y)
            print("goal_polygon_token:", goal_polygon_token)
            ego_dubinspose_token = self.get_dubinspose_token_from_polygon \
                (observation, (self._ego_x, self._ego_y, self._ego_yaw), ego_polygon_token)
            print("ego_dubinspose_token:", ego_dubinspose_token)
        else:
            ego_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._goal_x,
                                                                                                    self._goal_y)
            ego_polygon_id = int(ego_polygon_token.split('-')[1])
            print("ego_polygon_token:", ego_polygon_token)
            goal_polygon_token = observation['hdmaps_info']['tgsc_map'].get_polygon_token_using_node(self._ego_x,
                                                                                                     self._ego_y)
            print("goal_polygon_token:", goal_polygon_token)
            ego_dubinspose_token = self.get_dubinspose_token_from_polygon \
                (observation, (self._goal_x, self._goal_y, self._goal_yaw), ego_polygon_token)
            print("ego_dubinspose_token:", ego_dubinspose_token)
        #################获取目标车最匹配的dubinspose#################

        # ego_dubinspose_id = int( ego_dubinspose_token.split('-')[1])
        # ego_dubinspose_token 作为 起点、终点的path拿到
        link_referencepath_tokens_ego_polygon = observation['hdmaps_info']['tgsc_map'].polygon[ego_polygon_id][
            'link_referencepath_tokens']
        # 去除掉 不包含 ego_dubinspose_token 的 path
        for _, path_token in enumerate(link_referencepath_tokens_ego_polygon):
            path_id = int(path_token.split('-')[1])
            link_dubinspose_tokens = observation['hdmaps_info']['tgsc_map'].reference_path[path_id][
                'link_dubinspose_tokens']
            if ego_dubinspose_token not in link_dubinspose_tokens:
                pass
            else:
                only_one_path_token = path_token
                only_one_path_id = path_id
        print("path_token:", path_token)
        path_connected = []
        for road in main_road[goal_polygon_token]:
            if only_one_path_token in road:
                ind = road.index(only_one_path_token)
                terminal_path_id = int(road[ind].split('-')[1])
                terminal_path = observation['hdmaps_info']['tgsc_map'].reference_path[terminal_path_id]['waypoints']
                if bo_in:
                    _, nearest_point_ind, _, _ = self.find_nearest_point(terminal_path, [self._ego_x, self._ego_y])
                    terminal_path = terminal_path[nearest_point_ind:]
                    road_1 = road[ind + 1:len(road)]
                    for road_segment in road_1:
                        path_id = int(road_segment.split('-')[1])
                        path_connected += observation['hdmaps_info']['tgsc_map'].reference_path[path_id]['waypoints']
                    path_connected = terminal_path + path_connected
                else:
                    _, nearest_point_ind, _, _ = self.find_nearest_point(terminal_path, [self._goal_x, self._goal_y])
                    terminal_path = terminal_path[0:nearest_point_ind + 1]
                    road_1 = road[:ind]
                    for road_segment in road_1:
                        path_id = int(road_segment.split('-')[1])
                        path_connected += observation['hdmaps_info']['tgsc_map'].reference_path[path_id]['waypoints']
                    path_connected = path_connected + terminal_path
        return path_connected

    def get_dubinspose_token_from_polygon(self, observation, veh_pose: Tuple[float, float, float], polygon_token: str):
        id_polygon = int(polygon_token.split('-')[1])
        link_dubinspose_tokens = observation['hdmaps_info']['tgsc_map'].polygon[id_polygon]['link_dubinspose_tokens']
        dubinsposes_indicators = []
        for token in link_dubinspose_tokens:
            id_dubinspose = int(token.split('-')[1])
            dx = observation['hdmaps_info']['tgsc_map'].dubins_pose[id_dubinspose]['x'] - veh_pose[0]
            dy = observation['hdmaps_info']['tgsc_map'].dubins_pose[id_dubinspose]['y'] - veh_pose[1]
            dyaw = observation['hdmaps_info']['tgsc_map'].dubins_pose[id_dubinspose]['yaw'] - veh_pose[2]
            distance = math.sqrt(dx ** 2 + dy ** 2)
            if distance < 3 and abs(dyaw) < 1e-2:
                return token

    def line_equation(self, x, y, angle):
        if angle != np.pi / 2 and angle != -np.pi / 2:  # 避免斜率为无限大的情况
            m = np.tan(angle)
            b = y - m * x
            return m, b
        else:
            return float('inf'), x  # 对于垂直线，斜率为无限大，返回x作为截距

    def calculate_curvature(self, x1, y1, angle1, x2, y2, angle2):
        # 中点坐标
        # mid_x, mid_y = (x1 + x2) / 2.0, (y1 + y2) / 2.0

        # 如果两个方向角度相同，曲线不是圆弧，是直线
        if angle1 == angle2:
            return 0  # 直线的曲率为0，倒数为无穷大

        # 计算垂直于两点连线的直线的斜率（即中垂线）
        # 避免除以零的错误
        # if x2 != x1:
        #     slope_perpendicular = -(x2 - x1) / (y2 - y1)
        # else:
        #     slope_perpendicular = float('inf')

        # 使用点斜式方程计算两条直线的方程
        # y = mx + b，通过一个点和斜率求b
        # 计算两个方向的直线方程
        m1, b1 = self.line_equation(x1, y1, angle1)
        m2, b2 = self.line_equation(x2, y2, angle2)

        # 找到圆心（两条直线的交点）
        if m1 != float('inf') and m2 != float('inf'):
            cx = (b2 - b1) / (m1 - m2)
            cy = m1 * cx + b1
        elif m1 == float('inf'):
            cx = b1
            cy = m2 * cx + b2
        else:
            cx = b2
            cy = m1 * cx + b1

        # 计算圆心到任一点的距离（半径）
        radius = np.sqrt((cx - x1) ** 2 + (cy - y1) ** 2)

        # 计算曲率
        curvature = 1 / radius

        return curvature

    def find_nearest_point(self, path, point):
        """
        在路径上找到给定点的最近点。

        参数:
        - path: 路径点的列表，每个点是一个(x, y)元组。
        - point: 给定点，一个(x, y)元组。

        返回:
        - nearest_point: 路径上最近的点。
        - min_distance: 到给定点的最小距离。
        """

        # 初始化最小距离和最近点
        min_distance = float('inf')
        nearest_point = None
        point_ind = 0
        # print(path)
        # 遍历路径上的每个点
        for i in range(len(path)):
            path_point = path[i]
            # 计算当前点与给定点之间的欧几里得距离
            distance = math.sqrt((path_point[0] - point[0]) ** 2 + (path_point[1] - point[1]) ** 2)

            # 更新最小距离和最近点
            if distance < min_distance:
                point_ind = i
                nearest_point = path_point
                min_distance = distance
        if nearest_point == None:
            flag = True
        else:
            flag = False
        return nearest_point, point_ind, min_distance, flag



